//
// Created by liz on 24-11-8.
//
#include "he_serial_servo.h"
#include "he_serial_servo_cmd.h"
#include "bsp_usart.h"
#include <bsp_dwt.h>
#include "bsp_log.h"
#include "user_lib.h"

#include "memory.h"
#include "stdlib.h"
// #include "daemon.h"

#define GET_LOW_BYTE(A) ((uint8_t)(A))
//宏函数 获得A的低八位
#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8))
//宏函数 获得A的高八位

#define SET_AS_LOW_BYTE(A) (A & 0xFF)
#define SET_AS_HIGH_BYTE(A) ((A >> 8) & 0xFF00)

#define SERVO_MAX_POS 1000
#define SERVO_MIN_POS 0

#define SERVO_MAX_ANGLE 240
#define SERVO_MIN_ANGLE 0

//把0-1000转换为0-240.0度
#define RECEIVEPOS2ANGLE(POS) ((POS - SERVO_MIN_POS)/(float)(SERVO_MAX_POS-SERVO_MIN_POS)*(SERVO_MAX_ANGLE-SERVO_MIN_ANGLE))
//把0-240.0度转为0-1000
#define ANGLE2POS(ANGLE) ((ANGLE-SERVO_MIN_ANGLE)/(float)(SERVO_MAX_ANGLE-SERVO_MIN_ANGLE)*(SERVO_MAX_POS-SERVO_MIN_POS))

static ServoInstance *servo_motor_instance[SERVO_MOTOR_CNT];
static uint8_t servo_idx = 0; // register servo_idx,是该文件的全局舵机索引,在注册时使用

static void DecodeServo(USARTInstance *_handle);

// // 所有舵机只用单个串口，使用单例模式不封装
// static USARTInstance *he_serial_servo_instance;

ServoInstance *HE_ServoInit(Servo_Init_Config_s *init_config) {
    ServoInstance *servo = (ServoInstance *)malloc(sizeof(ServoInstance));
    memset(servo, 0, sizeof(ServoInstance));

    servo->servo_type = init_config->servo_type;
    servo->servo_id = init_config->servo_id;


    switch (init_config->servo_type)
    {
        case SERVO_BUS:
            USART_Init_Config_s config;
            config.module_callback = DecodeServo;
            config.recv_buff_size = SERVO_MAX_BUFF;
            config.usart_handle = init_config->usart_handle;
            config.tx_fifo_mode = USART_FIFO_ENABLE;
            config.duplex_mode = USART_DUPLEX_HALF;
            servo->usart_instance = USARTRegister(&config);
        break;
        case SERVO_PWM:
            servo->angle_max = init_config->angle_max;
            servo->angle_min = init_config->angle_min;

            servo->pwm_instance = PWMRegister(&init_config->pwm_init_config);
            PWM_SetDuty(servo->pwm_instance,0.075f);
        break;
        default:
            LOGERROR("Servo type error");
        break;
    }

    servo_motor_instance[servo_idx] = servo;
    servo_idx++;


    return servo;
}



static inline uint8_t serial_servo_checksum(const uint8_t buf[])
{
    uint16_t temp = 0;
    for (int i = 2; i < buf[3] + 2; ++i) {
        temp += buf[i];
    }
    return (uint8_t)(~temp);
}

/**
* @brief: HE_ServoSetAngle: 按角度值设置舵机角度
* @param[in]: servo: 舵机实例
* @param[in]: angle: 角度值
* @param[in]: move_time: 移动时长参数
* @param[in]: synchronous: 同步标志位
* @retval: 无
* @details:
*
**/
void HE_ServoSetAngle(ServoInstance *servo, float angle,uint16_t move_time,uint8_t synchronous) {
    static float servo_dt;
    static float servo_start;
    servo_start=DWT_GetTimeline_ms();

    switch(servo->servo_type)
    {
        case SERVO_BUS:
            angle=float_constrain(angle,0,240);
            static uint8_t txBuff[10];
            SerialServoCmdTypeDef frame={0};
            int16_t target_pos = ANGLE2POS(angle);
            target_pos = (target_pos > 1000) ? 1000 : target_pos;

            frame.header_1=SERIAL_SERVO_FRAME_HEADER;
            frame.header_2=SERIAL_SERVO_FRAME_HEADER;

            frame.elements.servo_id=servo->servo_id;
            frame.elements.length = 7;
            frame.elements.command=(synchronous)?SERIAL_SERVO_MOVE_TIME_WAIT_WRITE : SERIAL_SERVO_MOVE_TIME_WRITE;
            frame.elements.args[0] = GET_LOW_BYTE(target_pos);
            frame.elements.args[1] = GET_HIGH_BYTE(target_pos);
            frame.elements.args[2] = GET_LOW_BYTE(move_time);
            frame.elements.args[3] = GET_HIGH_BYTE(move_time);

            memcpy(txBuff,&frame,10);

            //计算CRC
            txBuff[9]=serial_servo_checksum(txBuff);

            USARTSend(servo->usart_instance,txBuff,frame.elements.length + 3,USART_TRANSFER_DMA);
            break;
        case SERVO_PWM:
            static float current_angle = 0; // 当前角度
            static float last_update_time = 0; // 上次更新的时间
            static float max_rate_of_change = 10; // 最大变化速率（单位：角度/毫秒）

            // 限制角度在合理范围内
            angle = float_constrain(angle, servo->angle_min, servo->angle_max);

            // 计算当前时间和时间间隔
            float current_time = DWT_GetTimeline_ms();
            float delta_time = current_time - last_update_time;

            // 计算期望的最大变化角度
            float max_change = max_rate_of_change * delta_time;

            static float delta_angle;
            // 计算角度变化的步长，确保不会超过最大变化速率
            delta_angle = angle - current_angle;
            // if (delta_angle > max_change) {
            //     delta_angle = max_change;
            // } else if (delta_angle < -max_change) {
            //     delta_angle = -max_change;
            // }
            delta_angle=abs_limit(delta_angle,max_change);

            // 更新当前角度
            current_angle += delta_angle;

            // 计算PWM占空比并更新PWM输出
            float target_duty = (current_angle - servo->angle_min) / (servo->angle_max - servo->angle_min) * 0.1 + 0.025;
            PWM_SetDuty(servo->pwm_instance, target_duty);

            // 更新上次更新时间
            last_update_time = current_time;
            break;
        }
    servo_dt = DWT_GetTimeline_ms() - servo_start;
}

void HE_ServoRead(ServoInstance *servo, SERVO_READ_TYPE read_type) {
    static uint8_t txBuff[10];
    SerialServoCmdTypeDef frame={0};

    frame.header_1=SERIAL_SERVO_FRAME_HEADER;
    frame.header_2=SERIAL_SERVO_FRAME_HEADER;

    frame.elements.servo_id=servo->servo_id;
    frame.elements.length = 3;
    frame.elements.command=read_type+SERIAL_SERVO_TEMP_READ;

    memcpy(txBuff,&frame,frame.elements.length+3);

    txBuff[frame.elements.length+2]=serial_servo_checksum(txBuff);

    USARTSend(servo->usart_instance,txBuff,frame.elements.length + 3,USART_TRANSFER_DMA);

}

//@todo 只读取了角度 还有电压，动作是否完成等 且只支持一个串口
void DecodeServo(USARTInstance *_handle)
{
    //先从触发回调的串口中获取消息并检查
    uint8_t *recv_buff = _handle->recv_buff;
    //检验包头
    if (recv_buff[0] == SERIAL_SERVO_FRAME_HEADER && recv_buff[1] == SERIAL_SERVO_FRAME_HEADER) {
        for (uint8_t i = 0; i < servo_idx; i++)
        {
            if(servo_motor_instance[i]->servo_id==recv_buff[2]) {
                switch (recv_buff[4]) {
                    case SERIAL_SERVO_MOVE_TIME_READ:
                        break;
                    case SERIAL_SERVO_MOVE_TIME_WAIT_READ:
                        break;
                    case SERIAL_SERVO_TEMP_READ:
                        servo_motor_instance[i]->servo_temp = recv_buff[5];
                        break;
                    case SERIAL_SERVO_VIN_READ:
                        servo_motor_instance[i]->servo_vin = SET_AS_LOW_BYTE(recv_buff[5]) | SET_AS_HIGH_BYTE(recv_buff[6]);
                        break;
                    case SERIAL_SERVO_POS_READ:
                        servo_motor_instance[i]->current_pos = SET_AS_LOW_BYTE(recv_buff[5]) | SET_AS_HIGH_BYTE(recv_buff[6]<<8);
                        servo_motor_instance[i]->angle = RECEIVEPOS2ANGLE(servo_motor_instance[i]->current_pos);
                        break;
                    case SERIAL_SERVO_LOAD_OR_UNLOAD_READ:
                        servo_motor_instance[i]->servo_en = SET_AS_LOW_BYTE(recv_buff[5]);
                        break;

                    default:
                        break;

                }

                return;
            }



        }
    }

}